/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

//this is an alternate main to be used when compiling without wxwidgets

#include <unistd.h>
#include <string>
#include <GL/glut.h>

#include "ownutils.h"
#include "owndebug.h"
#include "World.h"
#include "MLMA.h"
#include "mathutils.h"
#include "GLProvider.h"

#define ANGLE_INCR 2.0;
#define DIST_INCR 0.1

#define Z_INITIAL -1.5
#define Y_INITIAL 1.5
#define X_INITIAL 0
#define PAN_INITIAL 0
#define TILT_INITIAL 20

#define DESIRED_FPS 30

static int windowID=0; //main window id


static float x,y,z,pan,tilt;

void DisplayFunc();
void ReshapeFunc(int x, int y);

struct windowInfo
{
  int id;
  int w, h;
};

static std::vector<windowInfo> windowInfos;

class Provider : public CGLProvider
{
  public:
    Provider()
    {}
    
    ~Provider(){}

    int CreateNewContext(int w, int h, const char* name)
    {
      int id;
      windowInfo i;
      i.w=w;
      i.h=h;
      i.id=glutCreateWindow(name);
      
      glutSetWindow(i.id);
      
      glutDisplayFunc(DisplayFunc);
      //glutIdleFunc(IdleFunc);
      //glutSpecialFunc(KeyboardFunc);
      glutReshapeFunc(ReshapeFunc);
      
      glutReshapeWindow(w,h);
      windowInfos.push_back(i);
      return i.id;
    }

    bool SetContext(int id)
    {
      glutSetWindow(id);
      return glutGetWindow();
    }
    
    void ResetContext()
    {
      glutSetWindow(windowID);
    }


  void GetCameraParams(float& aX,
                       float& aY,
                       float& aZ,
                       float& aPan,
                       float& aTilt)
  {
    //printf("Giving camera parameters!\n");

    aX = x;
    aY = y;
    aZ = z;
    aPan = pan;
    aTilt = tilt;
    
  }


    
  private:
  
} provider;



static bool run=true;

static bool parse=true;
static int option;
static int debuglevel=1;
static int stepsize_ms=20;
static int simmode=1;
static std::string logfile="log.txt";
static std::string simfile;
std::string gimiSettingsFile="gimi_settings.xml";
std::string hubAddress="";
int hubPort=-1;
bool useGimiSettingsFile=true;
static bool simfileSet=false;
static bool graphics=true;
static bool drawgrid=false;

static double lastRepaint=0;

ownThreadHandle thread;
ownMutexHandle worldMutex;

static CWorld *world=NULL;



bool LoadWorld()
{
  world=MLMALoadWorld(simfile.c_str());

  if (!world)
    return false;

  world->SetStepSize(stepsize_ms/1000.0);
  world->SetQuickMode(simmode<0);

  world->SetDrawGridMode(drawgrid);
  
  return true;
}

void ResetView()
{
  z=Z_INITIAL;
  y=Y_INITIAL;
  x=X_INITIAL;
  pan=PAN_INITIAL;
  tilt=TILT_INITIAL;
}

void PrintHelp()
{
  printf("Usage:\n");
  printf("FSRSim <world file name> <options>\n"); 
  printf("Possible options:");
  printf("-v <level> - set debug level\n");
  printf("-s <step size> - set step size in milliseconds\n");
  printf("-q - quick mode\n");
  printf("-x - disable graphics\n");
  printf("-L <file name> - log target file\n");
  printf("-f <conf_file> - Use 'conf_file' as configuration file, or optionally use following parameters:\n");
  printf("-u <addr> - Use 'addr' as GIMnet AccessPoint address\n");
  printf("-p <port> - Use 'port' as GIMnet AccessPoint port\n");
  printf("-g - enable XY grid\n");
}


void SetView()
{

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glRotatef(pan,0,1,0);
  glRotatef(tilt,cos(DegToRad(pan)),0,sin(DegToRad(pan)));
  glTranslatef(x,-y,z);


}

static void *ThreadEntry(void *param)
{

  double start, diff, step, speed;
  int sleepTime=1;

  if (!LoadWorld())
  {
    exit(true);
  }
  
  if (useGimiSettingsFile)
  {
    printf("Using file %s for gimi connection settings.\n",gimiSettingsFile.c_str());
    world->SetGimiSettingsFile(gimiSettingsFile);
  } else {
    printf("Using hub %s, port %d as gimi acces point.\n",hubAddress.c_str(),hubPort);
    world->SetHub(hubAddress,hubPort);
  }
  
  if (graphics)
    world->SetGLProvider(&provider);
  
  world->Activate();
  
  while (run)
  {
    start=ownTime_get_s();
    step=world->GetStepSize();

    ownMutex_Lock(worldMutex);
    world->Step(); //simulate
    ownMutex_Unlock(worldMutex);
    
    diff=ownTime_get_s_since(start);
    speed=step/diff;

    if (diff<step)
    {
      sleepTime=(int)(1000.0*(step-diff));
      speed=1.0;
    } else {
      sleepTime=1;
    }

    ownSleep_ms(sleepTime);
  }
  return NULL;
}

void DrawHUD()
{
  
}

/********************* glut callbacks **************************/

void DisplayFunc()
{
  if (glutGetWindow()==windowID)
  {
    SetView();
    
    if (world!=NULL)
    {
      ownMutex_Lock(worldMutex);
      world->Draw();
      ownMutex_Unlock(worldMutex);
    }
    
    /*for (std::vector<windowInfo>::iterator i= windowInfos.begin();
         i!=windowInfos.end(); i++)
    {
      glutSetWindow(i->id);
      glutPostRedisplay();
    }
    glutSetWindow(windowID);*/
  }
  glutSwapBuffers();
}

void IdleFunc()
{
  if (ownTime_get_s_since(lastRepaint)>1.0/DESIRED_FPS)
  {
    lastRepaint=ownTime_get_s();
    glutSetWindow(1);
    glutPostRedisplay();
  } else {
    ownSleep_ms(5);
  }
}

void KeyboardFunc(int key, int mouse_x, int mouse_y)
{
  
  float distStep=DIST_INCR;
  float angleStep=ANGLE_INCR;
  
  if (glutGetModifiers()==GLUT_ACTIVE_SHIFT)
  {
    distStep*=3;
    angleStep*=3;
  }
  
  switch (key)
    {
      case GLUT_KEY_UP :
      //z++;
        x=x-sin(DegToRad(pan))*distStep;
        z=z+cos(DegToRad(pan))*distStep;
        break;
      case GLUT_KEY_DOWN:
        //z--;
        x=x+sin(DegToRad(pan))*distStep;
        z=z-cos(DegToRad(pan))*distStep;
        break;
      case GLUT_KEY_RIGHT:
        pan=pan+angleStep;
        break;
      case GLUT_KEY_LEFT:
        pan=pan-angleStep;
        break;
      case GLUT_KEY_PAGE_UP:
        y=y+distStep/2.0;
        break;
      case GLUT_KEY_PAGE_DOWN:
        y=y-distStep/2.0;
        break;
      case GLUT_KEY_HOME:
        tilt=tilt-angleStep;
        break;
      case GLUT_KEY_END:
        tilt=tilt+angleStep;
        break;
      case GLUT_KEY_INSERT:
        tilt=0;
        break;
    }
}

void ReshapeFunc(int w, int h)
{
  int window=glutGetWindow();
  if (window==windowID)
  {
    glMatrixMode(GL_PROJECTION);
    glViewport(0, 0, w, h);
    glLoadIdentity();
    gluPerspective(80,1.0*w/h,0.1,1000);
  } else {
    dPrint(1,"camera window resized");
    for (std::vector<windowInfo>::iterator i= windowInfos.begin();
        i!=windowInfos.end(); i++)
    {
      if (i->id == window)
      {
        dPrint(1,"setting viewport for win %d", i->id);
        glMatrixMode(GL_PROJECTION);
        glViewport(0, 0, i->w, i->h);
        return;
      }
    }
  }
}


/******************* main *******************/

int main(int argc, char *argv[])
{  
  
  
  debugInit();
  

  printf("\nFSRSim\n\n");

  while (parse)
  {
    option=getopt(argc,argv,"v:s:L:qxghf:u:p:");
    switch (option)
    {
      case 'v':
        if (optarg != NULL)
          debuglevel=atoi(optarg);
        break;
      case 's':
        if (optarg != NULL)
          stepsize_ms=atoi(optarg);
        break;
      case 'q':
        simmode=-1;
        break;
      case 'L':
        if (optarg != NULL)
          logfile = optarg;
        break;
      case 'x':
        graphics = false;
        break;
      case 'g':
        drawgrid = true;
        break;
      case 'h':
        PrintHelp();
        exit(true);
        break;
      case 'f':
        if (optarg != NULL)
        {
          useGimiSettingsFile=true;
          gimiSettingsFile = optarg;
        }
        break;
      case 'u':
        if (optarg != NULL)
        {
          useGimiSettingsFile=false;
          hubAddress= optarg;
        }
        break;
      case 'p':
        if (optarg != NULL)
        {
          useGimiSettingsFile=false;
          hubPort= atoi(optarg);
        }
        break;
      case -1:
        parse = false;
        break;
    }
  }
  if (optind < argc)
  {
    simfile=argv[optind];
    simfileSet=true;

  }

  debugSetGlobalDebugLvl(debuglevel);
  //debugSetMode(DEBUG_MODE_FORMAT_FILE_FUNC_LINE);
  debugSetLogFilename(logfile.c_str());
  //debugSetLogFilename("log.txt");
  debugAddMode(DEBUG_MODE_IMMEDIATE_FLUSH);
  
  if (!simfileSet)
  {
    PrintHelp();
    exit(true);
  }
  
  
  
  if (graphics)
  { 
    glutInit(&argc,argv);
    glutInitDisplayMode( GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
    glutInitWindowSize(400,300);
      
    ResetView();
  
    windowID=glutCreateWindow("FSRSim");
    glutSetWindow(windowID);
    
    
    
    glutDisplayFunc(DisplayFunc);
    glutIdleFunc(IdleFunc);
    glutSpecialFunc(KeyboardFunc);
    glutReshapeFunc(ReshapeFunc);
  }
  
  
  worldMutex=ownMutex_Init();
  thread=ownThread_Create((void*)ThreadEntry,NULL);
  
  if (!graphics) 
  {
    printf("Running without graphics\n");
    while (1)
    {
      ownSleep_ms(1000);
    }
  } else {
    glutMainLoop();
  }
}
